
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "algebra.h"

#include <util/common_macros.h>
#include <util/math_macros.h>
#include <util/msg.h>
#include <util/types.h>


#include "../matrix/matrix.h"

#include "alg_primitives.h"

// =============================================================================

void imp_algebra_SVD(ImpMatrix * A, ImpMatrix ** U, ImpMatrix ** S, ImpMatrix ** V) {

	if (*U == NULL) (*U) = imp_matrix_new(A->rows, A->rows);
	if (*S == NULL) (*S) = imp_matrix_new(1, A->columns);
	if (*V == NULL) (*V) = imp_matrix_new(A->columns, A->columns);

	svd(A->rows, A->columns, A->data, (*S)->data, (*U)->data, (*V)->data);

}

// =============================================================================

void imp_algebra_QR_GramSchmidt(ImpMatrix * A, ImpMatrix ** Q, ImpMatrix ** R) {

	int r, c, k;
	real sq_len;
	real sum_proj;
	real prod_in;

	int rows;
	int columns;
	int inner_dim;
	real * _A;
	real * _Q;
	real * _R;

	rows = A->rows;
	columns = A->columns;
	inner_dim = MIN(A->rows, A->columns);

	if (Q == NULL) exit(EXIT_FAILURE);
	else if ((*Q) == NULL) {
		(*Q) = imp_matrix_new(A->rows, inner_dim);
	}
	if (R == NULL) exit(EXIT_FAILURE);
	else if ((*R) == NULL) {
		(*R) = imp_matrix_new(inner_dim, A->columns);
	}

	_A = A->data;
	_Q = (*Q)->data;
	_R = (*R)->data;

	for (c = 0; c < inner_dim; c++) {
		// For each columns, do:

		sq_len = 0.0;
		for (r = 0; r < rows; r++) {
			sum_proj = 0.0;
			for (k = 0; k < c; k++) {
				sum_proj += _R[ k * columns + c ] * _Q[ r * inner_dim + k ];
			}
			_Q[ r * inner_dim + c ] = _A[ r * columns + c ] - sum_proj;
			sq_len += _Q[ r * inner_dim + c ] * _Q[ r * inner_dim + c ];
		}
		prod_in = 0.0;
		for (r = 0; r < rows; r++) {
			_Q[ r * inner_dim + c ] /= sqrt(sq_len);
			prod_in += _Q[ r * inner_dim + c ] * _A[ r * columns + c ];
		}
		_R[ c * columns + c ] = prod_in;

		// In this point, q_c is ready. Compute the inner product between q_c and all a_k, k > c and k < columns
		for (k = c + 1; k < columns; k++) {
			prod_in = 0;
			for (r = 0; r < rows; r++) {
				prod_in += _Q[ r * inner_dim + c ] * _A[ r * columns + k ];
			}
			_R[ c * columns + k ] = prod_in;
		}
	}
}

void imp_algebra_append_right(ImpMatrix * mleft, ImpMatrix * mright, ImpMatrix ** mdest) {

	int r, c;

	_exit_if_fail(mleft->rows == mright->rows);
	_exit_if_fail(mdest);

	if (*mdest == NULL) (*mdest) = imp_matrix_new(mleft->rows, mleft->columns + mright->columns);

	/* copy Left to M*/
	for (r = 0; r < mleft->rows; r++) {
		for (c = 0; c < mleft->columns; c++) {
			(*mdest)->data[ r * (*mdest)->columns + c ] = mleft->data[ r * mleft->columns + c];
		}
	}
	/* append Right into the right side of M */
	for (r = 0; r < mright->rows; r++) {
		for (c = 0; c < mright->columns; c++) {
			(*mdest)->data[ r * (*mdest)->columns + (c + mleft->columns) ] = mright->data[ r * mright->columns + c];
		}
	}
}

void imp_algebra_transpose(ImpMatrix * A, ImpMatrix ** T) {

	int r, c;

	_exit_if_fail(T);

	if (*T == NULL) (*T) = imp_matrix_new(A->columns, A->rows);

	for (r = 0; r < A->rows; r++) {
		for (c = 0; c < A->columns; c++) {
			(*T)->data[ c * (*T)->columns + r ] = A->data[ r * A->columns + c ];
		}
	}
}

void imp_algebra_sum(ImpMatrix * A, ImpMatrix ** S) {

	int c, r, _r, idx;
	real * _S;
	real * _A;

	_exit_if_fail(S);
	if (*S == NULL) (*S) = imp_matrix_new(A->rows, A->columns);

	_S = (*S)->data;
	_A = A->data;

	for (r = 0; r < A->rows; r++) {
		_r = r * A->columns;
		for (c = 0; c < A->columns; c++) {
			idx = _r + c;
			_S[c] += _A[idx];
		}
	}
}

void imp_algebra_add(ImpMatrix * A, ImpMatrix * B, ImpMatrix ** S) {
	int idx;

	real * _S;
	real * _A;
	real * _B;

	_exit_if_fail(S);
	if (*S == NULL) (*S) = imp_matrix_new(A->rows, A->columns);

	_S = (*S)->data;
	_A = A->data;
	_B = B->data;

	for (idx = 0; idx < A->size; idx++) {
		_S[idx] = _A[idx] + _B[idx];
	}
}

void imp_algebra_sub(ImpMatrix * A, ImpMatrix * B, ImpMatrix ** Diff) {
	int idx;

	real * _A;
	real * _B;
	real * _D;

	_exit_if_fail(Diff);
	if ((*Diff) == NULL) (*Diff) = imp_matrix_new(A->rows, A->columns);

	_A = (real*) A->data;
	_B = (real*) B->data;
	_D = (real*) (*Diff)->data;

	for (idx = 0; idx < A->size; idx++) {
		_D[idx] = A->data[idx] - B->data[idx];
	}
}

void imp_algebra_pow(ImpMatrix * A, real pot) {
	long int i;

	_exit_if_fail(A);

	for (i = 0; i < A->size; i++) {
		A->data[i] = pow(A->data[i], pot);
	}
}

void imp_algebra_mult(ImpMatrix * A, ImpMatrix * B, ImpMatrix ** Mult) {
	int r, c, k;
	real sum;

	real * _M;
	real * _A;
	real * _B;

	_exit_if_fail(Mult);
	if (*Mult == NULL) (*Mult) = imp_matrix_new(A->rows, B->columns);

	_M = (*Mult)->data;
	_A = A->data;
	_B = B->data;

	for (r = 0; r < A->rows; r++) {
		for (c = 0; c < B->columns; c++) {
			sum = 0.0;
			for (k = 0; k < A->columns; k++) {

				sum += _A[ r * A->columns + k] * _B[ k * B->columns + c];
			}
			_M[ r * (*Mult)->columns + c] = sum;
		}
	}
}

void imp_algebra_dot(ImpMatrix * A, ImpMatrix * B, ImpMatrix ** Dot) {
	int idx;

	real * mdot;
	real * ma;
	real * mb;

	_exit_if_fail(Dot);
	if (*Dot == NULL) (*Dot) = imp_matrix_new(A->rows, A->columns);

	mdot = (*Dot)->data;
	ma = A->data;
	mb = B->data;

	for (idx = 0; idx < A->size; idx++) {
		mdot[idx] = ma[idx] * mb[idx];
	}
}

void imp_algebra_diag(ImpMatrix * M) {

	int r, c, ci;
	real k;

	int cols;
	int rows;

	real * A = M->data;

	cols = M->columns;
	rows = M->rows;

	for (c = 0; c < (cols - 1); c++) {
		r = rows - 1;
		for (; r > c; r--) {

			if (A[ r * cols + c ] == 0) continue;

			k = A[ (r - 1) * cols + c ] / A[ r * cols + c ];

			for (ci = 0; ci < cols; ci++) {
				A[ r * cols + ci ] = k * A[ r * cols + ci ] - A[ (r - 1) * cols + ci ];
			}
		}
	}
}

void imp_algebra_upper_triangle(ImpMatrix * A, ImpMatrix ** Upper) {
	int w, h, _h;
	real * data;
	int p;
	real P, k;
	int ip;
	int columns;
	int rows;

	_exit_if_fail(Upper);
	if (*Upper == NULL) *Upper = imp_matrix_new_copy(A);
	else imp_matrix_copy(A, Upper);

	columns = A->columns;
	rows = A->rows;
	data = (*Upper)->data;

	for (p = 0; p < columns && p < rows; p++) {
		/* Select the PIVOT */
		P = data[p * columns + p];

		ip = p * columns;

		for (h = p + 1; h < rows; h++) {
			_h = h * columns;

			if (data[ h * columns + p ] == 0) continue;

			/* Calculate the multiply factor */
			k = (-1) * data[ h * columns + p ] / P;

			for (w = 0; w < columns; w++) {
				//idx = _h + w;
				data[_h + w] = data[ _h + w] + k * data[ ip + w ];
			}
		}
	}

	/*
		for (h = 0; h < rows; h++) {
			P = data[h * columns + h];
			_h = h * columns;
			for (w = h; w < columns; w++) {
				idx = _h + w;
				data[idx] = data[idx] / P;
			}
		}
	 */
}

real imp_algebra_determinant(ImpMatrix * A) {
	real det;
	ImpMatrix * upper = NULL;
	int p;

	if (A->rows != A->columns) {
		msg_error("ERROR: ImpMatrix is not square!\n");
		exit(EXIT_FAILURE);
	}

	imp_algebra_upper_triangle(A, &upper);

	det = 1.0;
	for (p = 0; p < upper->size; p += upper->columns + 1) {
		det *= upper->data[p];
	}

	imp_object_delete(upper);
	return det;
}

/**
 * Retorna a matriz inversa da matriz quadrada <code>A</code>. Utiliza o
 * método de Gauss-Jordan (matriz aumentada).
 *
 * @param A
 * @param inv
 */
void imp_algebra_inv_GaussJordan(ImpMatrix * A, ImpMatrix ** inv) {

	int h, w, _h, idx, p, ip;
	int dim;
	real * data;
	real * dinv;
	real P, k;

	ImpMatrix * temp;

	_exit_if_fail(A->rows == A->columns);
	_exit_if_fail(inv);

	if (*inv == NULL) (*inv) = imp_matrix_new_identity(A->rows);
	else {
		int i;
		for (i = 0; i < A->rows; i++) {
			(*inv)->data[i * A->rows + i] = 1.0;
		}
	}

	temp = imp_matrix_new_copy(A);

	dim = A->columns;
	data = temp->data;
	dinv = (*inv)->data;

	/* Upper Triangle */
	for (p = 0; p < dim; p++) {

		P = data[p * dim + p];
		ip = p * dim;

		for (h = p + 1; h < dim; h++) {

			_h = h * dim;

			if (data[h * dim + p] == 0) continue;
			k = (-1) * P / data[h * dim + p];

			for (w = 0; w < dim; w++) {

				idx = _h + w;
				data[ idx ] = k * data[ idx ] + data[ ip + w];
				dinv[ idx ] = k * dinv[ idx ] + dinv[ ip + w];
			}
		}
	}
	/* Lower Triangle */
	for (p = dim - 1; p >= 0; p--) {

		P = data[p * dim + p];
		ip = p * dim;

		for (h = p - 1; h >= 0; h--) {

			_h = h * dim;

			if (data[h * dim + p] == 0) continue;
			k = (-1) * P / data[h * dim + p];

			for (w = 0; w < dim; w++) {

				idx = _h + w;
				data[ idx ] = k * data[ idx ] + data[ ip + w];
				dinv[ idx ] = k * dinv[ idx ] + dinv[ ip + w];
			}
		}
	}

	for (p = 0; p < dim; p++) {
		P = data[p * dim + p];
		for (w = 0; w < dim; w++) {
			data[p * dim + w] /= P;
			dinv[p * dim + w] /= P;
		}
	}
	imp_object_delete(temp);
}

/**
 * 
 * 
 * @param A input matrix
 * @param R the reduced matrix A in the row echelon form
 * @param Ind independent variables
 * @param N null space matrix
 * @param rank rank of the matrix A
 */
void imp_algebra_rref(ImpMatrix * A, ImpMatrix ** R, ImpMatrix ** Ind, ImpMatrix ** N, int * rank) {

	int columns;
	int rows;
	int w, h, pw, ph, pi, i, c;
	real P, k;
	real tmp;

	ImpMatrix * RR = NULL;
	ImpMatrix * NN = NULL;
	ImpMatrix * Indp = NULL;

	if (R == NULL) {
		// Doesn't return R
		RR = imp_matrix_new_copy(A);
	} else {
		if ((*R) == NULL) RR = imp_matrix_new_copy(A);
		else RR = (*R);
	}

	Indp = imp_matrix_new(1, A->columns);

	*rank = 0;
	columns = A->columns;
	rows = A->rows;

	// For each pivot, do:
	for (ph = pw = 0; pw < columns && ph < rows; pw++, ph++) {
		pi = ph * columns + pw;

		Indp->data[pw] = 0.0;

		// select the pivot element
		P = RR->data[pi];

		// <editor-fold defaultstate="collapsed" desc="Row Exchange if needed">
		if (ABS(P) == 0.0) {
			// if pivot is zero, then we have to find a non-zero
			// element below it and perform the row exchange

			for (h = ph; h < rows; h++) {
				// finding a non-zero element
				tmp = RR->data[h * columns + pw];
				if (ABS(tmp) != 0) {
					break;
				}
			}
			if (h == rows) {
				// There is no non-zero element below pivot.
				// Then we have to find a new pivot rightmost
				ph--;
				// ph is decremented because the loop (for) increment ph and pw
				// at the same time. Thus, the next increment will get a
				// rightmost index to pw.
				continue;
			} else {
				// There is a non-zero element below pivot in row h.
				// Then we have to perform row exchange between h and ph

				i = ph * columns;
				c = h * columns;
				for (w = pw; w < columns; w++) {
					tmp = RR->data[ i + w ];
					RR->data[ i + w ] = RR->data[ c + w ];
					RR->data[ c + w ] = tmp;
				}

				// Select the new pivot element
				P = RR->data[pi];
			}
		}// </editor-fold>

		(*rank)++;
		Indp->data[pw] = 1;

		// <editor-fold defaultstate="collapsed" desc="Normalize the row of pivot">
		// Normalize the row of pivot
		if (P != 1.0) {
			i = ph * columns;
			for (w = pw; w < columns; w++) {
				RR->data[ i + w ] *= 1.0 / P;
			}
			P = 1;
		}// </editor-fold>

		// <editor-fold defaultstate="collapsed" desc="Lower Elimination">
		// = Do the lower elimination ==========================================
		for (h = ph + 1; h < rows; h++) {

			// Get the value below pivot element
			k = RR->data[ h * columns + pw ];

			if (k == 0.0) {
				// If the below value is zero, then we have perform 
				// the elimination of the next row
				continue;
			}

			// We just normalize the pivot row. Thus, the factor k does not be 
			// divided by P, because P is 1. 
			// k = k / P;

			i = h * columns;
			c = ph * columns;
			for (w = pw; w < columns; w++) {
				// Row elimination
				RR->data[ i + w ] = RR->data[ i + w ] - k * RR->data[c + w];
			}
		}// </editor-fold>

		// <editor-fold defaultstate="collapsed" desc="Upper Elimination">
		// Do the upper elimination
		for (h = ph - 1; h >= 0; h--) {
			// Get the value above pivot element
			k = RR->data[ h * columns + pw ];
			if (k == 0.0) {
				// If the below value is zero, then we have perform 
				// the elimination of the above rowjust
				continue;
			}

			// We just normalize the pivot row. Thus, the factor k does not be 
			// divided by P, because P is 1. 
			// k = k / P;

			i = h * columns;
			for (w = pw; w < columns; w++) {
				// Row elimination
				RR->data[ i + w ] = RR->data[ i + w ] - k * RR->data[ph * columns + w];
			}
		}// </editor-fold>

	}

	// <editor-fold defaultstate="collapsed" desc="Null Space">
	if (N != NULL) {
		// Need to compute the Null space of matrix R

		if (*rank == columns) {
			// The rank is equal to number of columns (variables) 
			// The Null Space is only the vector 0.
			NN = NULL;
		} else {

			// Need to compute the Null space of matrix R
			int p, hr, wn, hn;

			NN = imp_matrix_new(RR->columns, RR->columns - *rank);

			hr = 0; // row R index

			p = 0; // aux index to identity matrix
			for (hn = 0; hn < columns; hn++) {
				// for each variable do
				if (Indp->data[hn] == 1.0) {
					// i is a independent variable
					wn = 0; // column N index
					for (w = 0; w < columns; w++) {
						if (Indp->data[w] == 0.0) {
							NN->data[hn * NN->columns + wn] = (-1) * RR->data[hr * columns + w];
							wn++;
						}
					}
					hr++;
				} else {
					// i is a dependent variable.
					NN->data[hn * NN->columns + p] = 1;
					p++;
				}
			}
		}
	}// </editor-fold>

	if (R != NULL) (*R) = RR;
	else if (RR != NULL) imp_object_delete(RR);

	if (N != NULL) (*N) = NN;
	else if (NN != NULL) imp_object_delete(NN);

	if (Ind != NULL) (*Ind) = Indp;
	else if (Indp != NULL) imp_object_delete(Indp);


	/*
		// Verify the returns
		if (N == NULL && ind == NULL) {
			// Don't need return anything
			//return rank;
		}
		if (N == NULL && ind != NULL) {
			// Need return only the independent matrix flags
	 *ind = indp;
		}
		if (N != NULL && ind == NULL) {
			// Need return only the Null Space
	 *N = NN;
			imp_object_delete(indp);
		}
		if (N != NULL && ind != NULL) {
			// Need return both
	 *ind = indp;
	 *N = NN;
		}
	 */

	// return the rank of matrix R
	//return rank;
}

void imp_algebra_solve_by_elimination(ImpMatrix * A, ImpMatrix ** x, ImpMatrix * b, ImpMatrix ** N) {

	// Idea: To solve the system, we will use the function imp_algebra_rref to perform 
	// the row elimination of the augmented matrix [ A | b ]. 
	// Thus, we get the matrix in the reduced row echelon form  [ R | x ], 
	// where x is the solution.

	ImpMatrix * M = NULL;
	ImpMatrix * R = NULL;
	ImpMatrix * idp;
	int rank;
	int r;

	if (A->rows > A->columns) {
		// if the number of rows is greater than columns there is 1 or no 
		// solutions. Must to be 
		// 
		printf("Warning: The number of rows is greater than the number of columns.\n");
		printf("         We must use the Least Squares Method to solve the system!\n");

		(*x) = NULL;
		(*N) = NULL;
		exit(EXIT_FAILURE);
		return;
	}
	{
		// else: if the number of rows is less or equal to the number of columns, 
		// then: 
		//		if they are equal: there is one solution 
		//		else: 
		//			if columns is greater than rows.
	}

	// Create the augmented matrix [ A | b ]
	imp_algebra_append_right(A, b, &M);
	//M = _Matrix.append_right(A, b);


	// Get the reduced row echelon form of the augmented matrix M. 
	// This operation solves the system [A | b]x = 0.
	imp_algebra_rref(M, &R, &idp, NULL, &rank);

	// Now we have to verify if exist one or more solutions.
	if (rank < A->columns) {
		// If rank is smaller than the number of columns of A, 
		// then there are infinitely many solutions. 
		// We have to get the NullSpace of A, solving Ax=0.

		printf("Warning: Rank deficient, rank = %d.\n", rank);
		imp_algebra_rref(A, NULL, NULL, N, &rank);
	}
	// else: The matrix A is full rank. There is an unique solution.


	if (x == NULL) exit(EXIT_FAILURE);
	else if ((*x) == NULL) (*x) = imp_matrix_new(A->columns, b->columns);
	else imp_matrix_clear(*x);

	// Getting the particular solution of b. This code extract the values of 
	// matrix M. It contains 
	int cc = 0;
	for (r = 0; r < idp->columns; r++) {

		if (idp->data[r] == 1) {
			int k;
			for (k = 0; k < (*x)->columns; k++) {
				(*x)->data[ r * (*x)->columns + k ] = R->data[ cc * R->columns + (k + A->columns) ];
			}
			cc++;
		}
	}
	// The complete solution is the particular solution of Ax=b and the null 
	// space of A.
}

void imp_algebra_least_squares(ImpMatrix * A, ImpMatrix ** x, ImpMatrix * b) {
	ImpMatrix * R;
	ImpMatrix * ATA = NULL;
	ImpMatrix * ATb = NULL;
	ImpMatrix * AT = NULL;
	ImpMatrix * ind = NULL;
	int rank;

	if (x == NULL) exit(EXIT_FAILURE);
	else if ((*x) == NULL) (*x) = imp_matrix_new(A->columns, b->columns);
	else imp_matrix_clear(*x);

	imp_algebra_transpose(A, &AT);
	ATA = imp_matrix_new(A->columns, A->columns);
	ATb = imp_matrix_new(A->columns, b->columns);

	imp_algebra_mult(AT, A, &ATA);
	imp_algebra_mult(AT, b, &ATb);

	imp_algebra_append_right(ATA, ATb, &R);
	//R = _Matrix.append_right(ATA, ATb);
	imp_algebra_rref(R, &R, &ind, NULL, &rank);

	int r;
	int cc = 0;
	for (r = 0; r < ind->columns; r++) {

		if (ind->data[r] == 1) {
			int k;
			for (k = 0; k < (*x)->columns; k++) {
				(*x)->data[ r * (*x)->columns + k ] = R->data[ cc * R->columns + (k + ATA->columns) ];
			}
			cc++;
		}
	}
	imp_object_delete(AT);
	imp_object_delete(ATA);
	imp_object_delete(ATb);
	imp_object_delete(ind);
	imp_object_delete(R);
}

void imp_algebra_QR(ImpMatrix * A, ImpMatrix ** Q, ImpMatrix ** R) {

	int r, c, k;
	real sq_len;
	real sum_proj;
	real prod_in;

	int rows;
	int columns;
	int inner_dim;
	real * _A;
	real * _Q;
	real * _R;

	rows = A->rows;
	columns = A->columns;
	inner_dim = MIN(A->rows, A->columns);

	if (Q == NULL) exit(EXIT_FAILURE);
	else if ((*Q) == NULL) {
		(*Q) = imp_matrix_new(A->rows, inner_dim);
	}
	if (R == NULL) exit(EXIT_FAILURE);
	else if ((*R) == NULL) {
		(*R) = imp_matrix_new(inner_dim, A->columns);
	}

	_A = A->data;
	_Q = (*Q)->data;
	_R = (*R)->data;

	for (c = 0; c < inner_dim; c++) {
		// For each columns, do:

		sq_len = 0.0;
		for (r = 0; r < rows; r++) {
			sum_proj = 0.0;
			for (k = 0; k < c; k++) {
				sum_proj += _R[ k * columns + c ] * _Q[ r * inner_dim + k ];
			}
			_Q[ r * inner_dim + c ] = _A[ r * columns + c ] - sum_proj;
			sq_len += _Q[ r * inner_dim + c ] * _Q[ r * inner_dim + c ];
		}
		prod_in = 0.0;
		for (r = 0; r < rows; r++) {
			_Q[ r * inner_dim + c ] /= sqrt(sq_len);
			prod_in += _Q[ r * inner_dim + c ] * _A[ r * columns + c ];
		}
		_R[ c * columns + c ] = prod_in;

		// In this point, q_c is ready. Compute the inner product between q_c and all a_k, k > c and k < columns
		for (k = c + 1; k < columns; k++) {
			prod_in = 0;
			for (r = 0; r < rows; r++) {
				prod_in += _Q[ r * inner_dim + c ] * _A[ r * columns + k ];
			}
			_R[ c * columns + k ] = prod_in;
		}
	}
}

void imp_algebra_LU(ImpMatrix * A, ImpMatrix ** P, ImpMatrix ** L, ImpMatrix ** D, ImpMatrix ** U) {

	int r, c, p, _p, _r;
	int k;

	int dim;

	real Pivot;

	real tmp;
	real max;
	int r_of_max;

	real sum;


	int rows;
	int columns;
	int inner_dim;

	char flag_permutation_enable = 1;
	char flag_get_imp_matrix_D = 1;

	ImpMatrix * A_tmp;
	real * _A;
	real * _P;
	real * _L;
	real * _D;
	real * _U;

	rows = A->rows;
	columns = A->columns;
	inner_dim = MIN(A->rows, A->columns);


	if (L == NULL) exit(EXIT_FAILURE);
	else if ((*L) == NULL) (*L) = imp_matrix_new(A->rows, inner_dim);
	if (U == NULL) exit(EXIT_FAILURE);
	else if ((*U) == NULL) (*U) = imp_matrix_new(inner_dim, A->columns);

	if (P == NULL) flag_permutation_enable = 0;
	else if ((*P) == NULL) (*P) = imp_matrix_new(A->rows, A->rows);
	if (D == NULL) flag_get_imp_matrix_D = 0;
	else if ((*D) == NULL) (*D) = imp_matrix_new(inner_dim, inner_dim);


	/*
		if (P != NULL) (*P) = imp_matrix_new(A->rows, A->rows);
		(*L) = imp_matrix_new(A->rows, MIN(A->rows, A->columns));
		(*U) = imp_matrix_new(MIN(A->rows, A->columns), A->columns);
		if (D != NULL) (*D) = imp_matrix_new(inner_dim, inner_dim);
	 */


	A_tmp = imp_matrix_new_copy(A);
	_A = A_tmp->data;
	if (P != NULL) _P = (*P)->data;
	_L = (*L)->data;
	if (D != NULL) _D = (*D)->data;
	_U = (*U)->data;

	dim = MIN(A->rows, A->columns);

	if (flag_permutation_enable) {
		for (p = 0; p < (*P)->size; p += rows + 1) {
			_P[p] = 1;
		}
	}

	// Initialize the L matrix as identity matrices.
	for (p = 0; p < (*L)->size; p += (inner_dim + 1)) {
		_L[ p ] = 1;
	}

	for (p = 0; p < dim; p++) {
		// for each pivot do:
		_p = p * A->columns;
		Pivot = _A[ _p + p];

		// If we need do the matrix permutations
		if (flag_permutation_enable) {
			// We need do the row exchange if the pivot is not the max values of column. 
			// To this, we have to find the row with the max absolute value below the pivot.
			max = ABS(Pivot);
			r_of_max = -1;
			for (r = p + 1; r < A->rows; r++) {
				tmp = _A[r * dim + p];

				max = ABS(max);
				tmp = ABS(tmp);

				if (max < tmp) {
					max = tmp;
					r_of_max = r;
				}
			}
			//if (r_of_max == -1 && Pivot == 0.0 ) {
			if (max == Pivot) {
				// The Pivot is bigger and we don't need exchange the rows. 
				// Bus, if Pivot is equal to 0, then the matrix is singular.

				if (Pivot == 0.0) {
					// we didn't find a absolute value different of zero.
					*L = NULL;
					*U = NULL;
					return;
				}

			} else {
				// We found a absolute value different of zero. Then, make row 
				// exchange between p and r_of_max. 

				_p = p * A->columns;
				_r = r_of_max * A->columns;
				for (c = 0; c < A->columns; c++) {
					tmp = _A[ _p + c ];
					_A[ _p + c ] = _A[ _r + c ];
					_A[ _r + c ] = tmp;
				}

				// Do the same row exchange in the Permutations ImpMatrix
				_p = p * rows;
				_r = r_of_max * rows;
				for (c = 0; c < rows; c++) {
					tmp = _P[ _p + c ];
					_P[ _p + c ] = _P[ _r + c ];
					_P[ _r + c ] = tmp;
				}

				// Select the new Pivot value
				Pivot = _A[ _p + p];
			}
		}

		// In this point of the algorithm, we have a good pivot. 
		// Then, let's to calculate the L and U values.

		for (r = 0; r < A->rows; r++) {
			for (c = 0; c < A->columns; c++) {

				if (r <= c) {
					sum = 0.0;
					for (k = 0; k < r; k++) {
						sum += _L[ r * inner_dim + k ] * _U[ k * columns + c ];
					}
					_U[ r * columns + c ] = _A[ r * A->columns + c ] - sum;
				} else {
					sum = 0.0;
					for (k = 0; k < c; k++) {
						sum += _L[ r * inner_dim + k ] * _U[ k * columns + c ];
					}
					if (_U[c * columns + c] == 0) {
						/* 0 divide */
						continue;
					}
					_L[r * inner_dim + c] = (_A[r * A->columns + c] - sum) / _U[c * columns + c];
				}
			}
		}
	}

	if (flag_get_imp_matrix_D) {
		// Compute the D matrix
		for (p = 0; p < (*D)->columns; p++) {

			_p = p * (*D)->columns;
			_r = p * columns;

			sum = _U[ p * columns + p];
			_D[ _p + p] = _U[ _r + p];

			for (r = p; r < columns; r++) {
				_U[ _r + r ] /= _D[ _p + p];
			}
		}
	}
	imp_object_delete(A_tmp);
}

void imp_algebra_cofactor(ImpMatrix * A, ImpMatrix ** C, int _r, int _c) {
	int r, c;
	int cr, cc;

	if (A->rows != A->columns) exit(EXIT_FAILURE);

	if (A->rows == 1) exit(EXIT_FAILURE);

	if (C == NULL) exit(EXIT_FAILURE);
	else if ((*C) == NULL) (*C) = imp_matrix_new(A->rows - 1, A->columns - 1);

	for (r = 0, cr = 0; r < A->rows; r++) {

		if (r == _r) continue;

		for (c = 0, cc = 0; c < A->columns; c++) {

			if (c == _c) continue;

			(*C)->data[ cr * (*C)->columns + cc ] = A->data[ r * A->columns + c ];

			cc++;
		}
		cr++;
	}
}

void imp_algebra_cofactor_matrix(ImpMatrix * A, ImpMatrix ** C) {

	int r, c;
	int cr, cc;
	real det;
	ImpMatrix * tmp = NULL;

	if (A->rows != A->columns) exit(EXIT_FAILURE);
	if (A->rows == 1) exit(EXIT_FAILURE);

	if (C == NULL) exit(EXIT_FAILURE);
	else if ((*C) == NULL) (*C) = imp_matrix_new(A->rows, A->columns);

	tmp = imp_matrix_new(A->rows - 1, A->columns - 1);

	for (r = 0, cr = 0; r < A->rows; r++) {
		for (c = 0, cc = 0; c < A->columns; c++) {

			imp_algebra_cofactor(A, &tmp, r, c);
			det = imp_algebra_determinant(tmp);

			if ((r + c) % 2 != 0) det = -det;

			(*C)->data[ r * (*C)->columns + c ] = det;
		}
	}
	imp_object_delete(tmp);
}

void imp_algebra_max(ImpMatrix * Mat, ImpMatrix ** Max, ImpMatrix ** Ind) {

	int c, r, _r, i;
	real * mat;
	real * max;
	real * ind;

	int rows, columns;

	_exit_if_fail(Mat);
	_exit_if_fail(Max);
	_exit_if_fail(Ind);

	rows = Mat->rows;
	columns = Mat->columns;

	if (*Max == NULL) (*Max) = imp_matrix_new(1, columns);
	if (*Ind == NULL) (*Ind) = imp_matrix_new(1, columns);

	mat = Mat->data;
	max = (*Max)->data;
	ind = (*Ind)->data;

	for (i = 0; i < columns; i++) {
		max[i] = mat[i];
	}

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {

			i = _r + c;

			if (mat[i] > max[c]) {
				max[c] = mat[i];
				ind[c] = r;
			}
		}
	}
}

void imp_algebra_max_row(ImpMatrix * Mat, ImpMatrix ** Max, ImpMatrix ** Ind) {

	int c, r, _r, i;
	real * mat;
	real * max;
	real * ind;

	int rows, columns;

	_exit_if_fail(Mat);
	_exit_if_fail(Max);
	_exit_if_fail(Ind);

	rows = Mat->rows;
	columns = Mat->columns;

	if (*Max == NULL) (*Max) = imp_matrix_new(1, rows);
	if (*Ind == NULL) (*Ind) = imp_matrix_new(1, rows);

	mat = Mat->data;
	max = (*Max)->data;
	ind = (*Ind)->data;

	for (i = 0; i < rows; i++) {
		max[i] = mat[i * columns];
	}

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {

			i = _r + c;

			if (mat[i] > max[r]) {
				max[r] = mat[i];
				ind[r] = c;
			}
		}
	}
}

void imp_algebra_min(ImpMatrix * Mat, ImpMatrix ** Min, ImpMatrix ** Ind) {

	int c, r, _r, i;
	real * mat;
	real * min;
	real * ind;

	int rows, columns;

	_exit_if_fail(Mat);
	_exit_if_fail(Min);
	_exit_if_fail(Ind);

	rows = Mat->rows;
	columns = Mat->columns;

	if (*Min == NULL) (*Min) = imp_matrix_new(1, columns);
	if (*Ind == NULL) (*Ind) = imp_matrix_new(1, columns);

	mat = Mat->data;
	min = (*Min)->data;
	ind = (*Ind)->data;

	for (i = 0; i < columns; i++) {
		min[i] = mat[i];
	}

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {

			i = _r + c;

			if (mat[i] < min[c]) {
				min[c] = mat[i];
				ind[c] = r;
			}
		}
	}
}

void imp_algebra_min_row(ImpMatrix * Mat, ImpMatrix ** Min, ImpMatrix ** Ind) {

	int c, r, _r, i;
	real * mat;
	real * min;
	real * ind;

	int rows, columns;

	_exit_if_fail(Mat);
	_exit_if_fail(Min);
	_exit_if_fail(Ind);

	rows = Mat->rows;
	columns = Mat->columns;

	if (*Min == NULL) (*Min) = imp_matrix_new(1, rows);
	if (*Ind == NULL) (*Ind) = imp_matrix_new(1, rows);

	mat = Mat->data;
	min = (*Min)->data;
	ind = (*Ind)->data;

	for (i = 0; i < rows; i++) {
		min[i] = mat[i * columns];
	}

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {

			i = _r + c;

			if (mat[i] < min[r]) {
				min[r] = mat[i];
				ind[r] = c;
			}
		}
	}
}


void __imp_algebra_LU(ImpMatrix * A, ImpMatrix ** P, ImpMatrix ** L, ImpMatrix ** D, ImpMatrix ** U) {

	int r, c, p, _p, _r;
	int k;

	int dim;

	real Pivot;

	real tmp;
	real max;
	int r_of_max;

	real sum;


	int rows;
	int columns;
	int inner_dim;

	char flag_permutation_enable = 1;
	char flag_get_imp_matrix_D = 1;

	ImpMatrix * A_tmp;
	real * _A;
	real * _P;
	real * _L;
	real * _D;
	real * _U;

	rows = A->rows;
	columns = A->columns;
	inner_dim = MIN(A->rows, A->columns);


	if (L == NULL) exit(EXIT_FAILURE);
	else if ((*L) == NULL) (*L) = imp_matrix_new(A->rows, inner_dim);
	if (U == NULL) exit(EXIT_FAILURE);
	else if ((*U) == NULL) (*U) = imp_matrix_new(inner_dim, A->columns);

	if (P == NULL) flag_permutation_enable = 0;
	else if ((*P) == NULL) (*P) = imp_matrix_new(A->rows, A->rows);
	if (D == NULL) flag_get_imp_matrix_D = 0;
	else if ((*D) == NULL) (*D) = imp_matrix_new(inner_dim, inner_dim);


	/*
		if (P != NULL) (*P) = imp_matrix_new(A->rows, A->rows);
		(*L) = imp_matrix_new(A->rows, MIN(A->rows, A->columns));
		(*U) = imp_matrix_new(MIN(A->rows, A->columns), A->columns);
		if (D != NULL) (*D) = imp_matrix_new((*U)->rows, (*U)->rows);
	 */


	A_tmp = imp_matrix_new_copy(A);
	_A = A_tmp->data;
	if (P != NULL) _P = (*P)->data;
	_L = (*L)->data;
	if (D != NULL) _D = (*D)->data;
	_U = (*U)->data;

	dim = MIN(A->rows, A->columns);

	if (flag_permutation_enable) {
		for (p = 0; p < (*P)->size; p += (*P)->columns + 1) {
			_P[p] = 1;
		}
	}

	// Initialize the Permutation and L matrices as identity matrices.
	for (p = 0; p < (*L)->size; p += ((*L)->columns + 1)) {
		_L[ p ] = 1;
	}

	for (p = 0; p < dim; p++) {
		// for each pivot do:
		_p = p * A->columns;
		Pivot = _A[ _p + p];

		// If we need do the matrix permutations
		if (flag_permutation_enable) {
			// We need do the row exchange if the pivot is not the max values of column. 
			// To this, we have to find the row with the max absolute value below the pivot.
			max = ABS(Pivot);
			r_of_max = -1;
			for (r = p + 1; r < A->rows; r++) {
				tmp = _A[r * dim + p];

				max = ABS(max);
				tmp = ABS(tmp);

				if (max < tmp) {
					max = tmp;
					r_of_max = r;
				}
			}
			//if (r_of_max == -1 && Pivot == 0.0 ) {
			if (max == Pivot) {
				// The Pivot is bigger and we don't need exchange the rows. 
				// Bus, if Pivot is equal to 0, then the matrix is singular.

				if (Pivot == 0.0) {
					// we didn't find a absolute value different of zero.
					*L = NULL;
					*U = NULL;
					return;
				}

			} else {
				// We found a absolute value different of zero. Then, make row 
				// exchange between p and r_of_max. 

				_p = p * A->columns;
				_r = r_of_max * A->columns;
				for (c = 0; c < A->columns; c++) {
					tmp = _A[ _p + c ];
					_A[ _p + c ] = _A[ _r + c ];
					_A[ _r + c ] = tmp;
				}

				// Do the same row exchange in the Permutations ImpMatrix
				_p = p * (*P)->columns;
				_r = r_of_max * (*P)->columns;
				for (c = 0; c < (*P)->columns; c++) {
					tmp = _P[ _p + c ];
					_P[ _p + c ] = _P[ _r + c ];
					_P[ _r + c ] = tmp;
				}

				// Select the new Pivot value
				Pivot = _A[ _p + p];
			}
		}

		// In this point of the algorithm, we have a good pivot. 
		// Then, let's to calculate the L and U values.

		for (r = 0; r < A->rows; r++) {
			for (c = 0; c < A->columns; c++) {

				if (r <= c) {
					sum = 0.0;
					for (k = 0; k < r; k++) {
						sum += _L[ r * (*L)->columns + k ] * _U[ k * (*U)->columns + c ];
					}
					_U[ r * (*U)->columns + c ] = _A[ r * A->columns + c ] - sum;
				} else {
					sum = 0.0;
					for (k = 0; k < c; k++) {
						sum += _L[ r * (*L)->columns + k ] * _U[ k * (*U)->columns + c ];
					}
					if (U[c * A->columns + c] == 0) {
						/* 0 divide */
						continue;
					}
					_L[r * (*L)->columns + c] = (_A[r * A->columns + c] - sum) / _U[c * (*U)->columns + c];
				}
			}
		}
	}

	if (flag_get_imp_matrix_D) {
		// Compute the D matrix
		for (p = 0; p < (*D)->columns; p++) {

			_p = p * (*D)->columns;
			_r = p * (*U)->columns;

			sum = _U[ p * (*U)->columns + p];
			_D[ _p + p] = _U[ _r + p];

			for (r = p; r < (*U)->columns; r++) {
				_U[ _r + r ] /= _D[ _p + p];
			}
		}
	}
	imp_object_delete(A_tmp);
}

void __imp_algebra_QR(ImpMatrix * A, ImpMatrix ** Q, ImpMatrix ** R) {

	int r, c, k;
	real sq_len;
	real sum_proj;
	real prod_in;

	int rows;
	int columns;
	int inner_dim;
	real * _Q;
	real * _R;

	rows = A->rows;
	columns = A->columns;
	inner_dim = MIN(A->rows, A->columns);

	if (Q == NULL) exit(EXIT_FAILURE);
	else if ((*Q) == NULL) (*Q) = imp_matrix_new(A->rows, inner_dim);
	if (R == NULL) exit(EXIT_FAILURE);
	else if ((*R) == NULL) (*R) = imp_matrix_new(inner_dim, A->columns);

	_Q = (*Q)->data;
	_R = (*R)->data;

	for (c = 0; c < (*Q)->columns; c++) {
		// For each columns, do:

		sq_len = 0.0;
		for (r = 0; r < (*Q)->rows; r++) {
			sum_proj = 0.0;
			for (k = 0; k < c; k++) {
				sum_proj += (*R)->data[ k * (*R)->columns + c ] * (*Q)->data[ r * (*Q)->columns + k ];
			}
			(*Q)->data[ r * (*Q)->columns + c ] = A->data[ r * A->columns + c ] - sum_proj;
			sq_len += (*Q)->data[ r * (*Q)->columns + c ] * (*Q)->data[ r * (*Q)->columns + c ];
		}
		prod_in = 0.0;
		for (r = 0; r < (*Q)->rows; r++) {
			(*Q)->data[ r * (*Q)->columns + c ] /= sqrt(sq_len);
			prod_in += (*Q)->data[ r * (*Q)->columns + c ] * A->data[ r * A->columns + c ];
		}
		(*R)->data[ c * (*R)->columns + c ] = prod_in;

		// In this point, q_c is ready. Compute the inner product between q_c and all a_k, k > c and k < columns
		for (k = c + 1; k < (*R)->columns; k++) {
			prod_in = 0;
			for (r = 0; r < (*Q)->rows; r++) {
				prod_in += (*Q)->data[ r * (*Q)->columns + c ] * A->data[ r * A->columns + k ];
			}
			(*R)->data[ c * (*R)->columns + k ] = prod_in;
		}
	}
}